#ifndef __PSD_H__
#define __PSD_H__

const static int FLT_SIZE = 6;
const static int SENSOR_NUMS = 12;

using namespace std;

class ParkSensorDistance
{
public:
    ParkSensorDistance();
    ~ParkSensorDistance();

    void setSensorPosition(int position[SENSOR_NUMS]); // angles for 12 park sensors.

    void setDistanceFront(int psd1, int psd2, int psd3, int psd4, int psd5, int psd6);
    void setDistanceBack(int psd7, int psd8, int psd9, int psd10, int psd11, int psd12);

    bool lessThan(int position, float distance);
    bool isValid(int position);

    int getDistance(int position);

    bool findLeftWayOut(int sensorId, int safe_distance);
    bool findRightWayOut(int sensorId, int safe_distance);
    bool findWayOut(int leftSensorId, int rightSensorId, int safe_distance);

    void psd_filter();
    bool psd_danger(int sensorId);
    bool psd_side_danger(int sensorId);
    void psd_tick();

    void Avoid_Danger(void);

private:
    int min_distance(int id);

private:
    int raw_idx1;
    int raw_idx2;

public:
    int raw_distances[SENSOR_NUMS][FLT_SIZE];
    int filter_distances[SENSOR_NUMS];

    int sensor_position[SENSOR_NUMS];

    int psd_danger_timeout[SENSOR_NUMS];
    int psd_side_danger_timeout[SENSOR_NUMS];
    int psd_danger_count[SENSOR_NUMS];
    int psd_side_danger_count[SENSOR_NUMS];
};

#endif
